﻿#include "stdafx.h"
#include "PhysXObj.h"
#include "PhysicsManager.h"
#include "PhysicsTerrain.h"
#include "GraphicScriptBinder.h"

#include "DebugAssert.h"

_NAMESPACE_BEGIN

PhysXObj::PhysXObj()
{
	scene = PhysicsManager::Instance()->GetWorldScene();
	actor = NULL;
	renderNode = NULL;
}

// 更新图形部分
void PhysXObj::Update()
{
	NxShape *const * shapes = actor->getShapes();
	NxU32 nShapes = actor->getNbShapes();
	while (nShapes--)
	{
		NxMat34 pose = shapes[nShapes]->getGlobalPose();
		NxVec3 pos  = shapes[nShapes]->getGlobalPosition();
		NxMat33 M = pose.M;
		Matrix3 mat;
		Vector3 position;
		position.x = pos.x;
		position.y = pos.y;
		position.z = pos.z;

		for (int i=0; i<3; i++)
		{
			NxVec3 t = M.getColumn(i);
			mat.SetColumn(i,Vector3(t.x,t.y,t.z));
		}
		//if(nShapes >= this->shapes.size())
		//	break;//暂时测试
		if(this->shapes[nShapes]->GetRenderNode() == NULL)
			break;
		//this->shapes[nShapes]->GetRenderNode()->translate(mat,0,0,0);
		Quaternion quat;
		quat.FromRotationMatrix(mat);
		this->shapes[nShapes]->GetRenderNode()->setOrientation(quat);
		this->shapes[nShapes]->GetRenderNode()->setPosition(position);
	}
}

void PhysXObj::CreateActor(bool isDynamic /* = true*/)
{
	if(actor)
		return ;
	NxBodyDesc bodyDesc; 

	if(isDynamic)
	{
		if(!actorDesc.isValid())
		{
			bodyDesc.mass = 1.0;
			bodyDesc.massSpaceInertia = NxVec3(10,10,10);
		}
		else
			actorDesc.density = 1;

		actorDesc.body=&bodyDesc;
	}
	assert(actorDesc.isValid());
	actor=scene->createActor(actorDesc);
	while(!actor)
	{
		actor=scene->createActor(actorDesc);
	}

	for (UINT i=0;i<shapes.size();i++)
	{
		NxShape* s = actor->getShapes()[i];
		shapes[i]->SetShapePtr(s);
	}		
}

void PhysXObj::DelShape( NxShape* pshape )
{
	for (std::vector<PhysXPrimitive*>::iterator i=shapes.begin();i!=shapes.end();i++)
	{
		if((*i)->pshape==pshape)
		{
			delete(*i);
			shapes.erase(i);
			return;
		}
	}
	assert(!"找不到要删除的shape");
}

bool PhysXObj::FindShape( PhysXPrimitive* p )
{
	std::vector<PhysXPrimitive*>::iterator f=find(shapes.begin(),shapes.end(),p);
	return shapes.end()!=f;
}

void PhysXObj::RemoveShape( PhysXPrimitive* p )
{
	//PhysicsManager::Instance()->ReleaseShape(actor,p->pshape);
	//vector<PhysXPrimitive*>::iterator f=find(shapes.begin(),shapes.end(),p);
	//shapes.erase(f);
}

NxMat34 PhysXObj::GetPose()
{
	return actor->getGlobalPose();
}

void PhysXObj::Initial( CONST_REF(NxVec3) pos )
{
	CreateActor();
	SetPos(pos.x,pos.y,pos.z);
}

void PhysXObj::SetPos( float x,float y,float z )
{
	if(!actor)return;
	NxMat34 pose = GetPose();
	pose.t = NxVec3(x,y,z);
	SetPose(pose);
	//actor->setGlobalPosition(NxVec3(x,y,z));
}

void PhysXObj::SetPose( CONST_REF(NxMat34)pose )
{
	actor->setGlobalPose(pose);
}

void PhysXObj::AddAnyShape( boost::any& des )
{
	if (Cube_Desc* cube = boost::any_cast<Cube_Desc>(&des))
	{
		AddShape<CubeShape>(*cube);
	}
	else if (Capsule_Desc* cps = boost::any_cast<Capsule_Desc>(&des))
	{
		AddShape<CapsuleShape>(*cps);
	}
	else if (Sphere_Desc* sph = boost::any_cast<Sphere_Desc>(&des))
	{
		AddShape<SphereShape>(*sph);
	}
	else if (Plane_Desc* pln = boost::any_cast<Plane_Desc>(&des))
	{
		AddShape<PlaneShape>(*pln);
	}
	else if (Wheel_Desc* wel = boost::any_cast<Wheel_Desc>(&des))
	{
		AddShape<WheelShape>(*wel);
	}
}

NxVec3 PhysXObj::GetPos()
{
	return actor->getGlobalPosition();
}

void PhysXObj::SetMesh( int id,CONST_REF(string)name, CONST_REF(string)res )
{
	string nm = name;
	Entity* entity = OgreProperty::Instance()->sceneManager->createEntity( name.c_str(), res.c_str() );
	nm+="_Node";
	SceneNode* node = OgreProperty::Instance()->sceneManager->getRootSceneNode()->createChildSceneNode( nm.c_str() );
	node->attachObject(entity);
	
	SetPhysRenderNode(id,node);
}

void PhysXObj::ScaleMesh( int id, CONST_REF(Vector3)scales )
{
	for (std::vector<PhysXPrimitive*>::iterator i=shapes.begin(); i!=shapes.end(); ++i)
	{
		(*i)->GetRenderNode()->scale(scales);
	}
}

PhysXPrimitive* PhysXObj::GetShape( int i )
{
	return shapes[i];
}

void PhysXObj::Show( bool val )
{
	for (std::vector<PhysXPrimitive*>::iterator i=shapes.begin(); i!=shapes.end(); ++i)
	{
		(*i)->GetRenderNode()->setVisible(val);
	}
}

//////////////////////////////////////////////////////////////////////////

PhysXPrimitive::~PhysXPrimitive()
{
	pOwner->GetActor()->releaseShape(*pshape);//这里应该是在GetPhysicsResults之后运行
}

void PhysXPrimitive::SetShapePtr( NxShape* p )
{
	p->userData=this;
	p->setGroup(Shape_Group_Pushable);
}

PhysXPrimitive::PhysXPrimitive()
{
	pOwner = NULL;
	renderNode = NULL;
}
//////////////////////////////////////////////////////////////////////////

void CubeShape::Create( const Cube_Desc& desc,NxActorDesc& actorDesc,NxActor* pActor )
{
	boxDesc.dimensions=*(NxVec3*)&desc.size;
	boxDesc.dimensions/=2;
	boxDesc.localPose=desc.localPose;

	if(pActor)
		SetShapePtr(pActor->createShape(boxDesc));
	else
	{
		actorDesc.shapes.pushBack(&boxDesc); 
		assert(actorDesc.isValid());
	}		
}

void CubeShape::SetSize( float x,float y,float z )
{
	((NxBoxShape*)pshape)->setDimensions(NxVec3(x,y,z)/2);
}

NxVec3 CubeShape::GetSize()
{
	return ((NxBoxShape*)pshape)->getDimensions()*2;	
}

void CubeShape::SetShapePtr( NxShape* p )
{
	PhysXPrimitive::SetShapePtr(p);
	pshape=dynamic_cast<NxBoxShape*>(p);
	assert(pshape);
}

//////////////////////////////////////////////////////////////////////////

void SphereShape::Create( const Sphere_Desc& desc,NxActorDesc& actorDesc,NxActor* pActor )
{
	sphereDesc.radius=desc.radius;
	sphereDesc.localPose=desc.localPose;
	if(pActor)		
		SetShapePtr(pActor->createShape(sphereDesc));
	else
		actorDesc.shapes.pushBack(&sphereDesc); 
}

void SphereShape::SetRadius( float radius )
{
	((NxSphereShape*)pshape)->setRadius(radius);
}

float SphereShape::GetRadius()
{
	return ((NxSphereShape*)pshape)->getRadius();
}

void SphereShape::SetShapePtr( NxShape* p )
{
	PhysXPrimitive::SetShapePtr(p);
	pshape=dynamic_cast<NxSphereShape*>(p);
	assert(pshape);
}

//////////////////////////////////////////////////////////////////////////

void CapsuleShape::Create( const Capsule_Desc& desc,NxActorDesc& actorDesc,NxActor* pActor )
{
	capsuleDesc.radius=desc.radius;
	capsuleDesc.height=desc.height;
	capsuleDesc.localPose=desc.localPose;
	if(pActor)
		SetShapePtr(pActor->createShape(capsuleDesc));
	else
		actorDesc.shapes.pushBack(&capsuleDesc); 
}

void CapsuleShape::SetParam( float radius,float height )
{
	((NxCapsuleShape*)pshape)->setRadius(radius);
	((NxCapsuleShape*)pshape)->setHeight(height);
}

float CapsuleShape::GetHeight()
{
	return ((NxCapsuleShape*)pshape)->getHeight();
}

float CapsuleShape::GetRadius()
{
	return ((NxCapsuleShape*)pshape)->getRadius();
}

void CapsuleShape::SetShapePtr( NxShape* p )
{
	PhysXPrimitive::SetShapePtr(p);
	pshape=dynamic_cast<NxCapsuleShape*>(p);
	assert(pshape);
}

//////////////////////////////////////////////////////////////////////////

void WheelShape::Create( const Wheel_Desc& desc,NxActorDesc& actorDesc,NxActor* pActor )
{
	this->suspension=desc.wheelSuspension;

	static NxMaterial* wsm = NULL;
	if (!wsm)
	{
		NxMaterialDesc m;
		m.flags |= NX_MF_DISABLE_FRICTION;
		wsm = PhysicsManager::Instance()->GetWorldScene()->createMaterial(m);
	}
	wheelDesc.materialIndex = wsm->getMaterialIndex();		

	NxReal heightModifier = (desc.wheelSuspension + desc.wheelRadius) / desc.wheelSuspension;

	wheelDesc.localPose.t = desc.position;
	NxQuat q;
	q.fromAngleAxis(90, NxVec3(0,1,0));
	wheelDesc.localPose.M.fromQuat(q);

	wheelDesc.suspension.spring = desc.springRestitution*heightModifier;
	wheelDesc.suspension.damper = 0;//desc.springDamping*heightModifier;
	wheelDesc.suspension.targetValue = desc.springBias*heightModifier;

	wheelDesc.radius = desc.wheelRadius;
	wheelDesc.suspensionTravel = desc.wheelSuspension; 
	wheelDesc.inverseWheelMass = 0.1;	//not given!? TODO

	wheelDesc.lateralTireForceFunction.stiffnessFactor *= desc.frictionToSide;	
	wheelDesc.longitudalTireForceFunction.stiffnessFactor *= desc.frictionToFront;	

	if(pActor)
	{
		wheel = static_cast<NxWheelShape*>(pActor->createShape(wheelDesc));
		SetShapePtr(wheel);
	}
	else
	{
		actorDesc.shapes.pushBack(&wheelDesc); 
	}

	wheel_desc = desc;
}

void WheelShape::SetParam( float radius )
{
	((NxWheelShape*)pshape)->setRadius(radius);
}

float WheelShape::GetWidth()
{
	return wheelDesc.radius;
}

float WheelShape::GetRadius()
{
	return ((NxWheelShape*)pshape)->getRadius();
}

void WheelShape::SetShapePtr( NxShape* p )
{
	PhysXPrimitive::SetShapePtr(p);
	pshape=dynamic_cast<NxWheelShape*>(p);
	assert(pshape);
}

NxVec3 WheelShape::GetWheelPos()
{
	if (wheel)
	{
		return wheel->getLocalPosition();
	}
	else 
		return NxVec3(0,0,0);
}

void WheelShape::SetAngle( NxReal angle )
{
	if (wheel)
	{
		wheel->setSteerAngle(-angle);
		wheel->getActor().wakeUp(1000);
	}
	
}

void WheelShape::Tick( bool handbrake, NxReal motorTorque, NxReal brakeTorque, NxReal dt )
{
	//motorTorque *= 0.1f;
	brakeTorque *= 500.0f;
	if(handbrake && GetWheelFlag(NX_WF_AFFECTED_BY_HANDBRAKE))
		brakeTorque = 1000.0f;

	if (wheel)
	{
		if(GetWheelFlag(NX_WF_ACCELERATED)) 
			wheel->setMotorTorque(motorTorque);

		wheel->setBrakeTorque(brakeTorque);
		wheel->getActor().wakeUp(1000);
	}
}

NxReal WheelShape::GetRpm()
{
	if(wheel)
		return NxMath::abs(wheel->getAxleSpeed())/NxTwoPi * 60.0f;
	else 
		return 0;
}

void WheelShape::GetWheel()
{
	wheel = static_cast<NxWheelShape*>(pshape);
}

NxActor* WheelShape::GetTouchedActor()
{
	NxWheelContactData wcd;
	NxShape * s = wheel->getContact(wcd);	
	return s ? &s->getActor() : 0;
}

NxVec3 WheelShape::GetGroundContactPos() const
{
	NxVec3 v1 = wheel->getLocalPosition();
	NxVec3 v2 = NxVec3(0, -(this->wheel->getRadius()), 0);
	return v1+v2;
}

void WheelShape::SetLateralTireForce( NxReal force )
{
	NxTireFunctionDesc desc;
	desc.stiffnessFactor = force;
	if(wheel)
	{
		wheel->setLateralTireForceFunction(desc);
		wheel->getActor().wakeUp(1);
	}
}

NxReal WheelShape::GetLateralTireForce()
{
	if (wheel)
	{
		return wheel->getLateralTireForceFunction().stiffnessFactor;
	}
	else
		return 0;
}
//////////////////////////////////////////////////////////////////////////

void PlaneShape::Create( const Plane_Desc& desc,NxActorDesc& actorDesc,NxActor* pActor )
{
	planeDesc.normal=NxVec3(0,1,0);
	planeDesc.d = desc.pos.y;

	if(pActor)
		SetShapePtr(pActor->createShape(planeDesc));
	else
		actorDesc.shapes.pushBack(&planeDesc); 
}

void PlaneShape::SetShapePtr( NxShape* p )
{
	PhysXPrimitive::SetShapePtr(p);
	pshape=dynamic_cast<NxPlaneShape*>(p);
	p->setGroup(Shape_Group_Terrain);
	assert(pshape);
}

//////////////////////////////////////////////////////////////////////////

void PhysPlane::Create( CONST_REF(NxVec3) pos )
{
	Plane_Desc desc;
	desc.pos = pos;
	//物理平面一般不直接SetPos，而是从传进来的pos向量与平面法线数乘后设置为distance
	AddShape<PlaneShape>(desc);
	CreateActor(false);
	
	actor->setGroup(Actor_Group_Plane);
}

//////////////////////////////////////////////////////////////////////////

void PhysBox::Create( CONST_REF(NxVec3)pos, CONST_REF(NxVec3)size )
{
	Cube_Desc desc;	
	desc.size=size;
	AddShape<CubeShape>(desc);
	Initial(pos);
}

void PhysBox::ResertMeshSize()
{
	NxVec3 size = shapes[0]->GetAABB();
	AxisAlignedBox aabb = shapes[0]->GetRenderNode()->_getWorldAABB();
	Vector3 rendSize = aabb.getSize();
	//shapes[0]->GetRenderNode()->scale(size.x/rendSize.x,size.y/rendSize.y,size.z/rendSize.z);
}

//////////////////////////////////////////////////////////////////////////

void PhysSphere::Create( CONST_REF(NxVec3)pos, float radius )
{
	Sphere_Desc desc;	
	desc.radius=radius;		
	AddShape<SphereShape>(desc);
	Initial(pos);
}

//////////////////////////////////////////////////////////////////////////

void PhysCapsule::Create( CONST_REF(NxVec3)pos,float radius,float height )
{
	Capsule_Desc desc;
	desc.radius=radius;
	desc.height=height;	
	AddShape<CapsuleShape>(desc);
	Initial(pos);
}

//////////////////////////////////////////////////////////////////////////

void PhysTerrain::Create( CONST_REF(NxVec3) pos )
{
	Terrain_Desc desc;
	desc.height = 513;
	desc.width = 513;
	desc.nbRows = 513;
	desc.nbColumns = 513;
	//desc.size = NxVec3(512,0.06f,512);
	desc.size = NxVec3(512,0.6f,512);
	desc.pos = NxVec3(0,0,0);
	desc.nameHeightmap = string("OtherTerrain.png");
	AddShape<TerrainShape>(desc);
	Initial(pos);
}

void PhysTerrain::CreateFromOhm( CONST_REF(NxVec3) pos,CONST_REF(string) name ,int worldSizeX, int worldSizeZ)
{
	string filename = name /*+ "\\Terrain\\Page000x000.ohm"*/;//暂时写死

	//获取地图大小
	FILE* fp = fopen(filename.c_str(),"rb");
	assert(fp && "ohm不存在");

	int size;
	fseek(fp,0,SEEK_END);
	size = ftell(fp);
	int mapsize = sqrt((float)(size/sizeof(float)));
	
	fclose(fp);

	Terrain_Desc desc;
	desc.height = mapsize;
	desc.width = mapsize;
	desc.nbRows = mapsize;
	desc.nbColumns = mapsize;
	desc.size = NxVec3(worldSizeX,1,worldSizeZ);
	desc.pos = NxVec3(-(mapsize-1),0,(mapsize-1));
	desc.nameHeightmap = filename;

	AddShape<TerrainShape>(desc);
	Initial(NxVec3(pos.x-worldSizeX/2,0,pos.z-worldSizeZ/2));
}

_NAMESPACE_END